import os
import launch.action
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
import launch.event_handlers
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory



def generate_launch_description():

    # 获取 xacro 路径与 world 路径
    robot_name_in_model = "fishbot"
    urdf_tutorial_path = get_package_share_directory('fishbot_description')

    # 使用 os.path.join 拼接路径
    default_xacro_path = os.path.join(urdf_tutorial_path, 'urdf/fishbot/fishbot.urdf.xacro')
    default_world_path = os.path.join(urdf_tutorial_path, 'newworld/custom_room.world')

    # 声明模型路径参数
    action_declare_arg_mode_path = DeclareLaunchArgument(
        name='model',
        default_value=default_xacro_path,
        description='Absolute path to robot xacro file'
    )

    # 动态生成 robot_description 参数
    robot_description_content = Command(['xacro ', LaunchConfiguration('model')])
    robot_description = ParameterValue(robot_description_content, value_type=str)

    # 添加 joint_state_publisher 节点（发布 /joint_states）
    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        parameters=[{'use_sim_time': True}],  # 必须添加！
        name='joint_state_publisher',
        output='screen'
    )

    # 添加 robot_state_publisher 节点（发布 TF）
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[
            {'robot_description': robot_description},
            {'use_sim_time': True}
        ],
        output='screen'
    )

    # 添加 static_transform_publisher 节点（发布 base_footprint → base_link）
    static_tf_basefootprint_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_basefootprint_tf',
        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
        output='screen'
    )

    # 启动 Gazebo 模拟器
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={
            'world': default_world_path,
            'verbose': 'true'
        }.items()
    )

    # 加载机器人到 Gazebo 中
    spawn_robot = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic', 'robot_description', '-entity', 'demobot1'],
        output='screen'
    )
    # 加载并激活关节状态
    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','fishbot_joint_state_broadcaster'],
        output='screen'
    )
    #激活力控制俩轮子
    action_load_effort_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','fishbot_effort_controller'],
        output='screen'
    )

    #激活速度控制俩轮子
    action_load_velocity_controller = launch.actions.ExecuteProcess(
        cmd=['ros2','control','load_controller','--set-state','active','fishbot_diff_drive_controller'],
        output='screen'
    )


    return LaunchDescription([
        action_declare_arg_mode_path,
        joint_state_publisher_node,
        robot_state_publisher_node,
        static_tf_basefootprint_node,   # ✅ 添加这一行
        gazebo_launch,
        spawn_robot,
        #机器人加载结束后执行
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_robot,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_velocity_controller],
            )
        ),
    ])

